/*****  Source file  *****/
#include <iostream>

#include <Eigen/Eigen>

#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>

#include "getPose.h"

using namespace std;
using namespace cv;


vector<Point3f> get3DPoints(targetInfo &strTargetInfo)
{
    vector<Point3f> objPoints;
    // set coordinate system in the middle of the marker, with Z pointing out
    objPoints.push_back( Point3f(-strTargetInfo.markerLength / 2.f,  strTargetInfo.markerWidth / 2.f, 0));
    objPoints.push_back( Point3f( strTargetInfo.markerLength / 2.f,  strTargetInfo.markerWidth / 2.f, 0));
    objPoints.push_back( Point3f( strTargetInfo.markerLength / 2.f, -strTargetInfo.markerWidth / 2.f, 0));
    objPoints.push_back( Point3f(-strTargetInfo.markerLength / 2.f, -strTargetInfo.markerWidth / 2.f, 0));

    return objPoints;
}


// the input corners'order shoule be
//LefeUp -> RightUp -> RightDown -> LeftDown
void estimatePose( vector<Point2f> corners, Mat cameraMatrix, Mat distCoeffs, targetInfo &strTargetInfo)
{
    if(corners.size()!=4)
        return;

    vector<Point3f> odjPoints3d;
    odjPoints3d = get3DPoints(strTargetInfo);

    Vec3d rvecs, tvecs;
    // for each corners, calculate its pose
    solvePnP(odjPoints3d, corners, cameraMatrix, distCoeffs, rvecs, tvecs); //only 4 points

    Mat rMatrix = Mat(3,3,CV_64F);
    cv::Rodrigues(rvecs, rMatrix);

    Eigen::Matrix3d R;
    R << rMatrix.at<double>(0,0), rMatrix.at<double>(0,1), rMatrix.at<double>(0,2),
            rMatrix.at<double>(1,0), rMatrix.at<double>(1,1), rMatrix.at<double>(1,2),
            rMatrix.at<double>(2,0), rMatrix.at<double>(2,1), rMatrix.at<double>(2,2);

    Eigen::Vector3d eular_angle;
    eular_angle = R.eulerAngles(0,1,2);            //radian
    eular_angle = R.eulerAngles(0,1,2)*180.f/M_PI; //angle

    //cout<<"R :"<<eular_angle<<endl;
    //cout<<"t :"<<tvecs<<endl;

    strTargetInfo.R.thetaX = eular_angle[0];
    strTargetInfo.R.thetaY = eular_angle[1];
    strTargetInfo.R.thetaZ = eular_angle[2];
    strTargetInfo.t.x = tvecs[0];
    strTargetInfo.t.y = tvecs[1];
    strTargetInfo.t.z = tvecs[2];
}

/*****  Header file  *****/

#ifndef _GETPOSE_H_
#define _GETPOSE_H_

#include <iostream>

typedef struct _targetInfo
{
    struct{
        float thetaX;
        float thetaY;
        float thetaZ;
    }R;

    struct{
        float x;
        float y;
        float z;
    }t;

    float markerLength=0.1f;    //meter
    float markerWidth =0.1f;

}targetInfo;


std::vector<cv::Point3f> get3DPoints(targetInfo &strTargetInfo);

// the input corners'order shoule be
//LefeUp -> RightUp -> RightDown -> LeftDown
void estimatePose(std::vector<cv::Point2f> corners, cv::Mat cameraMatrix, cv::Mat distCoeffs, targetInfo &strTargetInfo);

#endif

（利用迭代优化的方法对solvePnP计算的结果进一步精确化，但是迭代也会花费更长的时间）
solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
                                  InputArray cameraMatrix, InputArray distCoeffs,
                                  OutputArray rvec, OutputArray tvec,
                                  bool useExtrinsicGuess = false, int iterationsCount = 100,
                                  float reprojectionError = 8.0, double confidence = 0.99,
                                  OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );




